
#include "../include/qp_solver.hpp"
#include "../include/em_utils.hpp"

#include <iostream>
#include <vector>
#include <ros/ros.h>
#include <Eigen/Dense>

using namespace std;

vector<double> calculateQuinticParams(const SLPoint &start_sl, const SLPoint &end_sl)
{
    double start_s = start_sl.s;
    double end_s = end_sl.s; 
    
    double start_s2 = pow(start_s, 2);
    double start_s3 = pow(start_s, 3);
    double start_s4 = pow(start_s, 4);
    double start_s5 = pow(start_s, 5);
    double end_s2 = pow(end_s, 2);
    double end_s3 = pow(end_s, 3);
    double end_s4 = pow(end_s, 4);
    double end_s5 = pow(end_s, 5);
    Eigen::MatrixXd A(6, 6);
    A << 1,    start_s,    start_s2,    start_s3,   start_s4,    start_s5,
         0,    1,          2*start_s,   3*start_s2, 4*start_s3,  5*start_s4,
         0,    0,          2,           6*start_s,  12*start_s2, 20*start_s3,
         1,    end_s,      end_s2,      end_s3,     end_s4,      end_s5,
         0,    1,          2*end_s,     3*end_s2,   4*end_s3,    5*end_s4,
         0,    0,          2,           6*end_s,    12*end_s2,   20*end_s3;
    Eigen::MatrixXd B(6, 1);
    B << start_sl.l, 
         start_sl.l_prime, 
         start_sl.l_dprime, 
         end_sl.l, 
         end_sl.l_prime, 
         end_sl.l_dprime;
    Eigen::VectorXd X = A.inverse() * B;

    vector<double> res;
    for (int i = 0; i < X.rows(); i++)
        res.push_back(X[i]);
    return res;
}

int main(int argc, char** argv) {
    vector<double> ref_x_1 = {0, 1, 2, 3, 4, 5, 5.5, 6, 6.5, 7};
    vector<double> ref_y_1 = {0, 0, 2, 4, 4, 4.5, 5, 6, 9, 12};
    vector<double> ref_x_2 = {0, 1, 2, 3, 4, 5, 5.5, 6, 6.5, 7};
    vector<double> ref_y_2 = {4, 3, 2.5, 4, 4, 4.5, 5, 6, 9, 12};

    QPSolver qp_solver(false);
    vector<Point> res;
    qp_solver.updateRefePoint(ref_x_1, ref_y_1);
    qp_solver.initFEMSmoothMat(10, 2, 3, 2, -0.3, 0.3);
    qp_solver.run(res);

    res.clear();
    qp_solver.updateRefePoint(ref_x_2, ref_y_2);
    qp_solver.initFEMSmoothMat(10, 2, 3, 2, -0.3, 0.3);
    qp_solver.run(res);
    cout << "---------------------------------------" << endl;
    ros::init(argc, argv, "timestamp_conversion_node");
    ros::NodeHandle nh;
    ros::Rate rate(10);
    for (int i = 0; i < 10; i++) {
        ros::Time cur_t = ros::Time::now();
        uint32_t sec = cur_t.sec;
        uint32_t msec = cur_t.nsec / 1e6;
        // 将时间转化为毫秒
        uint32_t cur_msec = (cur_t.sec - 1.7e9) * 1000 + msec;
        cout << sec << " " << msec << " " << cur_msec << endl;
        rate.sleep();
    }
    cout << "--------------- osqp 求解五次多项式问题测试 ---------------" << endl;
    vector<double> params;
    qp_solver.initQuinticPoly();
    qp_solver.runQuinticPoly(params);
    std::cout << "params.size: " << params.size() << std::endl;
    if (params.size() > 0) {
        vector<double>::iterator it = params.begin();
        while (it != params.end())
        {
            std::cout << *it << std::endl;
            it++;
        }
    }
    cout << "--------------- 用逆矩阵求五次多项式参数测试 ---------------" << endl;
    SLPoint start, end;
    start.s = 1;
    start.l = 1;
    end.s = 6;
    end.l = 2;
    vector<double> param_vec = calculateQuinticParams(start, end);
    for (int i = 0; i < param_vec.size(); i++)
        std::cout << param_vec[i] << std::endl;
    cout << "--------------- Eigen 元素幂运算测试 ---------------" << endl;
    // https://blog.csdn.net/qq_27806947/article/details/105208615
    ArrayXd A(10);
    for (int i = 0; i < 10; i++) {
        A[i] = i;
    }
    ArrayXd B(A.pow(2));
    cout << "B: "<< B.transpose() << endl;
    cout << B.matrix().transpose() * B.matrix() << endl;
    cout << "---------------- Eigen fill function -------------" << endl;
    Eigen::MatrixXd mat_1 = Eigen::MatrixXd::Ones(2, 3);
    mat_1.resize(3, 2);
    mat_1.fill(2);
    cout << mat_1 << endl;
    cout << "---------------- test -------------" << endl;
    double delta = 1.0;
    double tatol_s = 0;
    std::cout << tatol_s << " ";
    for (int i = 1; i < 20; i++) {
        if (i % 10 == 0 && i >= 10) {
            delta += 1;
        }
        tatol_s += delta;
        std::cout << tatol_s << " ";
    }
    std::cout << std::endl;
    cout << "---------------- calcu distance -------------" << endl;
    double x = 1, y = 0;
    Eigen::Vector2d vec1(2 - x, 1 - y);
    Eigen::Vector2d vec3(3 - x, 1 - y);
    Eigen::Vector2d vec2 = vec3 - vec1;
    double min_dis = 0;
    double dis1 = vec1.norm();
    double dis2 = vec3.norm();
    double dis3 = sqrt(pow(vec1.norm(), 2) - pow(vec1.dot(vec2), 2));
    std::cout << dis1 << " " << dis2 << " " << dis3 << std::endl;
    if ((vec1.dot(vec2) > 0 && vec3.dot(vec2) > 0) || 
        (vec1.dot(vec2) < 0 && vec3.dot(vec2) < 0)) {
        min_dis = min(dis1, dis2);
    } else 
        min_dis = dis3;
    std::cout << min_dis << std::endl;
}